#ifndef PID_H
#define PID_H
#include <iostream>

struct PID
{
    float Ref = 0.0f;
    float FeedBack = 0.0f;

    float Error = 0.0f;
    float DError = 0.0f;
    float DDError = 0.0f;
    float PreError = 0.0f;
    float PreDError = 0.0f;
    float IntegralError = 0.0f;

    float Kp = 40.0f;
    float Ki = 3.0f;
    float Kd = 0.1f;

    float MaxOutValue = 12000.0f;
    float MinOutValue = -12000.0f;

    float Out = 0.0f;
};

inline float PIDIncControl(PID *pid, const float fdb)
{
	std::cout << fdb << std::endl;
    pid->FeedBack = fdb;

    pid->Error = pid->Ref - pid->FeedBack;
    pid->DError = pid->Error - pid->PreError;
    pid->DDError = pid->DError - pid->PreDError;

    pid->PreError = pid->Error;
    pid->PreDError = pid->DError;

    pid->Out += (pid->Kp * pid->DError + pid->Ki * pid->Error / 10 + pid->Kd * pid->DDError);

    if(pid->Out > pid->MaxOutValue)
    {
        pid->Out = pid->MaxOutValue;
    }
    if(pid->Out < pid->MinOutValue)
    {
        pid->Out = pid->MinOutValue;
    }
	std::cout << pid->Out << std::endl;

    return pid->Out;
}

inline float PIDPosControl(PID *pid, const float fdb)
{
    //
    pid->FeedBack = fdb;

    //
    pid->Error = pid->Ref - pid->FeedBack;
    pid->IntegralError	+=	pid->Error;
    pid->DError = pid->Error - pid->PreError;

    pid->PreError = pid->Error;

    //
    pid->Out = (pid->Kp * pid->Error + pid->Ki * pid->IntegralError / 10 + pid->Kd * pid->DError * 100);

    //
    if(pid->Out > pid->MaxOutValue)
    {
        pid->Out = pid->MaxOutValue;
    }
    if(pid->Out < pid->MinOutValue)
    {
        pid->Out = pid->MinOutValue;
    }

    //
    return pid->Out;
}

#endif //PID_H
